import os
import launch
import launch.actions
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch import LaunchDescription
from ament_index_python import get_package_share_directory
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration

def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    wheeltec_slam_dir = get_package_share_directory('wheeltec_slam_toolbox')
    wheeltec_slam = IncludeLaunchDescription(
          PythonLaunchDescriptionSource(
               os.path.join(wheeltec_slam_dir, 'launch', 'online_sync.launch.py')
          ),
     )
    parameters=[{
        'queue_size': 20,
        'frame_id': 'camera_link',
        'use_sim_time': use_sim_time,
        'subscribe_depth': True,
        
        # RTABMAP核心参数
        'Reg/Strategy': '0',
        'Reg/Force3DoF': 'true',
        'RGBD/NeighborLinkRefining': 'true',
        'RGBD/ProximityBySpace': 'true',
        'RGBD/AngularUpdate': '0.01',
        'RGBD/LinearUpdate': '0.01',
        'RGBD/OptimizeFromGraphEnd': 'false',
        'Grid/FromDepth': 'true',
        'Grid/CellSize': '0.05',
        
        # 新增地图保存参数
        'Database/DatabasePath': os.path.join(
            get_package_share_directory('wheeltec_robot_slam'),
            'maps',
            'rtabmap.db'
        ),
    }]

    remappings=[
        ('odom', '/odom_combined'),
        ('rgb/image', '/camera/rgb/image_raw'),
        ('rgb/camera_info', '/camera/rgb/camera_info'),
        ('depth/image', '/camera/depth/image_raw'),
        ('depth/camera_info', '/camera/depth/camera_info')
    ]

    return LaunchDescription([
        # wheeltec_slam,
        # Set env var to print messages to stdout immediately
        SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'),

        # Launch arguments
        DeclareLaunchArgument(
            'use_sim_time', default_value='false',
            description='Use simulation (Gazebo) clock if true'),

        # Nodes to launch
        Node(
            package='rtabmap_ros', executable='rtabmap', output='screen',
            parameters=parameters,
            remappings=remappings,
            arguments=['-d']),
            


    ])
